#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi

def func1():
    scale = 1.0

    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_panda_arm_to_pose', anonymous=True)

    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "panda_arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)

    move_group.set_goal_position_tolerance(0.01)

    move_group.set_goal_orientation_tolerance(0.01)

    waypoints = []

    wpose = move_group.get_current_pose().pose
    wpose.position.z -= scale * 0.1  # 1 向上 (z)
    wpose.position.y += scale * 0  # 以及侧边 (y)
    
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.x += scale * 0  # 2 向前/向后 in (x)
    waypoints.append(copy.deepcopy(wpose))

    wpose.position.y -= scale * 0  # 3 侧边移动 (y)
    waypoints.append(copy.deepcopy(wpose))

    waypoints.pop(0)

    # 我们希望以 1 cm 的精度插值笛卡尔路径，
    # 这就是为什么我们在笛卡尔换算 compute_cartesian_path() 函数中将最大步长 eef_step 设置为 0.01 的原因。
    # 我们将 jump_threshold 指定为 0.0 以禁用跳跃阈值，即
    # 忽略检查关节空间中不可行的 jump 。这对本教程而言禁用跳跃阈值无妨。
    (plan, fraction) = move_group.compute_cartesian_path(
                                    waypoints,   # 要求经过的沿途路径点
                                    0.01,        # eef_step, 末端执行器步长
                                    0.0)         # jump_threshold

    # 注意: 注意，我们只是在进行规划，而不是要求 move_group 真的让机器人运动。
    # return plan, fraction

    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                               moveit_msgs.msg.DisplayTrajectory,
                                               queue_size=20)

    display_trajectory = moveit_msgs.msg.DisplayTrajectory()
    display_trajectory.trajectory_start = robot.get_current_state()
    display_trajectory.trajectory.append(plan)
    # 发布消息
    display_trajectory_publisher.publish(display_trajectory)
   

    move_group.execute(plan, wait=True)


def move_to_target_pose():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_panda_arm_to_pose', anonymous=True)

    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "panda_arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)
    move_group.set_goal_position_tolerance(0.01)

    move_group.set_goal_orientation_tolerance(0.01)

    target_pose = geometry_msgs.msg.Pose()
    target_pose.position.x = 0.49 # 设置目标位置的 x 坐标
    target_pose.position.y = 0 # 设置目标位置的 y 坐标
    target_pose.position.z = 1.23 # 设置目标位置的 z 坐标

    target_pose.orientation.x = 0.7
    target_pose.orientation.y = 0
    target_pose.orientation.z = -0.707
    target_pose.orientation.w = 0 # 设置目标姿态的四元数

    move_group.set_pose_target(target_pose)

    plan = move_group.go(wait=True)

    # if plan:
    #     rospy.loginfo("Panda arm moved to the target pose successfully!")
    # else:
    #     rospy.loginfo("Failed to move Panda arm to the target pose.")

def jointmove():

    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_panda_arm_to_pose', anonymous=True)

    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "panda_arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)
    move_group.set_goal_position_tolerance(0.01)

    move_group.set_goal_orientation_tolerance(0.01)

    # 我们可以从 planning group 中获取并调整各关节角度:
    joint_goal = move_group.get_current_joint_values()
    joint_goal[0] = 0
    joint_goal[1] = -pi/4
    joint_goal[2] = 0
    joint_goal[3] = -pi/2
    joint_goal[4] = 0
    joint_goal[5] = pi/3
    joint_goal[6] = 0

    # 使用关节值或位姿来调用 go 命令，
    # 在已经设置了 planning group 的目标位姿或或目标关节角度的情况下可以不带任何参数。
    move_group.go(joint_goal, wait=True)

    # 调用 ``stop()`` 以确保没有未完成的运动。
    move_group.stop()

if __name__ == '__main__':
    try:
        # move_to_target_pose()
        jointmove()
        # func1()
    except rospy.ROSInterruptException:
        pass